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1. Introduction 


The find-path problem is well known in robotics. Given an object with an initial 
location and orientation, a goal location and orientation, and a set of obstacles located in 
space, the problem is to find a continuous path for the object from the initial position to 
the goal position which avoids collisions with obstacles along the way. This paper presents 
a new representation for free space, as natural “freeways” between obstacles, and a new 
algorithm to solve find-path using that representation. The algorithm’s advantages over 
those previously presented are that it is quite fast and it finds good paths which generously 
avoid obstacles rather than barely avoiding them. It does not find possible paths in ex¬ 
tremely cluttered situations but it can be used to provide direction to more computationally 
expensive algorithms in those cases. 

1.1 Approaches to the Find-Path problem. 

A common approach to the find-path problem, used with varying degrees of sophistica¬ 
tion, is the configuration space approach. Lozano-Perez [5] gives a thorough mathematical 
treatment of configuration space. The idea is to determine those parts of free space which 
a reference point of the moving object can occupy without colliding with any obstacles. 
A path is then found for the reference point through this truly free space. Dealing with 
rotations turns out to be a major difficulty with the approach - requiring complex geometric 
algorithms which are computationally expensive. 

Conceptually one can view configuration space as shrinking the object to a point while 
at the same time expanding the obstacles inversely to the shape of the moving object. This 
approach works well when the moving object is not allowed to rotate. If it can rotate then 
the grown obstacles must be embedded in a higher dimensional space - one extra dimen¬ 
sion for each degree of rotational freedom. Furthermore the grown obstacles have non- 
planar surfaces, even when the original problem was completely polygonal or polyhedral. 
Typically implementors have approximated the grown obstacles in order to be able to deal 
with rotations. 




Moravec [6] had to solve the find-path problem in two dimensions. lie bounded all 
obstacles and the moving object by circles. Then the grown obstacles were all perpendicular 
cylinders and the problem could be projected back into two dimensions where rotations 
could be ignored. This method missed all paths that required rotational manouevering. 

Lozano-Perez [5] split the rotation range into a fixed number of slices, and within each 
slice bounded the grown obstacles by polyhedra. Earlier Udupa [8] had used much cruder 
bounding polyhedra in a similar way. 

Recently Brooks and Lozano-Perez [3] developed a method for computing more directly 
with the curved surfaces of the grown obstacles. The algorithm can be quite expensive 
although given enough time and space it can find a path if one exists. 

1.2 Good representations simplify Find-Path. 

The algorithm presented in this paper is based on a different idea; that of using rep¬ 
resentations of space which capture the essential effects of translating and rotating a body 
through space. Free space is represented as overlapping generalized cones since they are 
descriptions of swept volumes. The cones provide a high level plan for moving the object 
through space. The volume swept by the particular object as it is translated and rotated 
is characterized as a function of its orientation. Find-path then reduces to comparing the 
swept volume of the object with the sweepable volumes of free space. This is done by 
inverting the characterization of the volume swept by an object, to determine its valid 
orientations as it moves through a generalized cone. 

Throughout this paper we restrict our attention to the two-dimensional problem where 
the object to be moved is a convex polygon and the obstacles are represented as unions of 
convex polygons. 



2. Describing Free Space as Generalized Cones 


Generalized cones are a commonly used representation of volume for modelling ob¬ 
jects in artificial intelligence based computer vision systems. They were first introduced by 
Binford [2], A generalized cone is formed by sweeping a two dimensional cross section along 
a curve in space, called a spine, and deforming it according to a sweeping rule. 

In this paper we will consider a two dimensional specialization of generalized cones 
(although we will still refer to them as volumes). The spines will be straight and the cross 
sections will be line segments held perpendicular to the spine with left and right radii. 
The sweeping rule will independently control the magnitudes of the left and right radii as 
piecewise linear functions. 

Free space is represented as overlapping generalized cones. Natural “freeways”, elon¬ 
gated regions through which the object might be moved, arc represented as generalized 
cones. The generalized cones overlap at intersections of these natural freeways. Figure 1 
illustrates a few of the generalized cones describing the free space around two obstacles in 
a confined workspace. 

2.1 Candidate generalized cones. 

A representation of free space is constructed by examining all pairs of edges of obstacle 
polygons. If the edges define a natural “freeway” through space they are used to construct 
a generalized cone. 

Each edge has a “free” side and a “full” side, where the “free” side is the outside of the 
particular polygon with which it is associated. Each edge has an outward pointing normal, 
pointing into the “free” side. Two edges are accepted as defining a candidate generalized 
cone if they meet the following two requirements. (1.) At least one vertex of each edge 
should be on the “free” side of the other. (2.) The dot product of the outward pointing 
normals should be negative. The second condition ensures that the “free” sides of the two 



Figure 1. A few of the generalized cones generated by two obstacles and the workspace 
boundary. 




edges essentially face each other. Thus in this initial stage there is a complexity factor of 
0(n 2 ), where n is the number of edges of obstacle polygons. 

Given a candidate pair of edges, a spine is constructed for the generalized cone. It is the 
bisector of the space which is on the “free” side of both edges. (Thus if the edges are parallel 
the spine is parallel to them both, and equidistant, or if the edges are not parallel then the 
spine bisects the angle they form.) The generalized cone occupies the volume between the 
two defining edges. At each vertex of the two edges (if the vertex is on the “free” side of 
the other edge) the cone is extended parallel to the spine. 

The generalized cone so defined may not lie entirely in free space. There may be other 
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Figure 2. Slices of the generalized cone removed due to obstacles. 


obstacles which intersect it. Each obstacle is compared to the generalized cone. A polygon 
can be intersected with the cone in time O(n) where n is the number of edges of the polygon. 
If the intersection is empty then nothing further need be done. If not then the intersection 
is projected normally onto the the spine of the generalized cone. This is illustrated in figure 
2. Again this is an 0{n ) operation in the number of vertices (and hence edges). The result of 
comparing all obstacles to the generalized cone is a set of regions of the spine where there is 
no obstacle which intersects the cone in a slice normal to the spine. Each disjoint slice which 
includes parts of the original two edges is then accepted as a generalized cone describing 
part of free space. 


Clearly the complete operation of describing free space is at most 0(n 3 ) in the number 
of edges in the obstacle polygons. 
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Figure 3. A generalized cone defined by l, bi, b r , Si, s r , m and c. 
2.2 The representation used for generalized cones. 


Figure 3 shows the complete representation used for cones describing parts of free space. 
There is a straight spine, parameterized over the range t E [0, l ] where l is the length of the 
cone. If the sides of the cone are not parallel to the spine then t = 0 corresponds to the 
wider end. 

On both the left and right the maximal radii achieved over the length of the cone occurs 
at t = 0 and arc denoted 6/ and b r respectively, describing the “big” end of the cone. The 
minimal radii achieved occur at t — l and arc denoted s t and s r , describing the “small” end 
of the cone. 




If bi = si and b r — s r then the two sides of the cone are parallel to the spine. If not, then 
there is a symmetric thinning of the cone (which may start, and end, at different values of 


7 








t on the left and right) where the left and right radii of the thinning parts of the cone are 
both given by the expression mt -j- c where m and c are constants. Note that it is always 
the case that m < 0 and c > 0. 

In summary, the seven constants l, 6/, b r , si, s r , m and c completely specify the shape 
and size of the generalized cone. In addition its location and orientation must be determined 
concurrently with computing these parameters. 

3. Determining Legal Orientations 

Let the moving object be called A. It is a convex polygon and has vertices op 02 , ..., 
a n . Choose an origin and z and y axes. Let d, be the distance of vertex a; from the origin. 
For optimal performance of the find-path algorithm the origin should be chosen so that 
max i <i<ndi is minimized over possible origins. The direction of the z-axis can be chosen 
arbitrarily. 

Orientations relative to A are defined in terms of an angle relative to A’s z-axis. Thus 
a point’s angle relative to A is the angle made by a ray from the origin to the point, relative 
to the z-axis. A direction relative to A is the angle made by a vector in that direction with 
A’s z-axis. 

Let ? 7 i, ri 2 , ..., r] n be the angles of the vertices a\, a 2 , ..., a n . 

3.1 The radius function of a polygon. 

Consider first the problem of determining the volume swept by a polygon as it is moved 
in a straight line with fixed orientation. Let the object be moving in direction 0. The swept 
volume depends on the object’s cross section perpendicular to that angle. The cross section 
can be broken into two components; that on the left of the direction of movement, and that 
on the right. Figure 4 illustrates. 




Figure 4. Volume swept by an object during pure translation. 

Define a radius function R(£) to be the infimum of the distance from the origin that a 
line which is normal to a ray at angle £ can be without intersecting the interior of A. Figure 
5 illustrates. (The radius function is closely related to the support of a convex polygon - 
e.g. see [1].) The magnitudes of the left and right cross sections of figure 4 can then be 
denoted R(0 -f- tt/ 2) anc * — 7r /2) respectively. 

Figure 6 shows the geometric construction of f?(£) for a given object A and angle £. 
The darkened outline in figure 7 shows the radius function R in polar coordinates for the 
same object. Thus R can be defined by 

/?(£) = max di cos(£ — rji). 

l<i<n 


The major interest in functions of this form will be in their inverse images of intervals 
of the form (—oo, r]. Let 

= m<r}. 

Thus is the range of angles at which the cross section of object A is no more than r. 

The inverse image can be easily computed by using the two values possible for arccos(r/dj) 
for each i to form an interval containing r/i and subtracting it from the interval [0,27r]. 


We restrict our attention to moving objects which are convex polygons precisely because 
their radius functions can be so easily inverted. 
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If we write R a {£) — R{£ -f- a) then the legal orientations of A relative to the direction 
of its movement down the center of a strip of diameter d can be written as 

l = Ri /2 (d/2)nR± lt/2 (d/2). 

Note that while L may consist of more than one interval, the set of orientations taken by 
A, in some trajectory along the length of the cone, must form a connected set - i.e. it must 
be a single interval, and so must be contained in a single interval of JL 

Finally note that the sum of two radius functions has the same form as a radius function, 
and so the “_L” operation can be computed just as easily on the sums of such functions as 
on single functions. I.e. with R as above and 

S(£) = max e; cos(£ — Hi) 

1 

then 


[fl + s](6) =/?(£) 4- S($) — max (d t cos(£ — m) -f e j C os(£ — /*,•)) 
and each term inside the “max” can be written in the form fa cos(£ — j/ tJ ). 

3.2 Bounding polygons with an appropriate rectangle. 

In general we wish to find the legal orientations for object A when its origin is at a 
point with spine parameter t on the spine of a generalized cone C. A conservative set of 
orientations can be found by enclosing /l in a rectangle which has two of its sides parallel to 
the spine of C. When A is oriented so that the spine has angle 9 relative to it, the bounding 
rectangle has the following properties. Its extent forward (in the direction of increasing spine 
parameter) from the origin is R{0). Its extents left and right are R(9 -f- 7 t/ 2) and R(9 — tt/2) 
respectively. Its extent to the rear is (rather conservatively) d = maxj<,< n d t -. Figure 8 
illustrates. The problem now is to invert this bounding operation - i.e. to find for what 
range of 0 the bounding rectangle is within the generalized cone. That is for each t £ [0,/] 
we wish to find a range of valid 9, denoted V(t) for orientations of the object A. 






Figure 8. An object A is bounded by a rectangle aligned with the spine of cone C. 

The rear of the bounding rectangle simply implies V(t) — 0 outside of the interval 
[d,/]. The forward bound implies 

V{t) Q R^(l - t). 

The parallel sides of the “big” part of the generalized cone imply upper bounds on the width 
of the bounding rectangle. Thus 

V(t) C (R±(b,) - ir/ 2) n (RHbr) + n/2). 

Note that the terms on the right involve adding a constant to a subset of [0, 2n] in the 
obvious way. Let V' be defined as 

V'{t) = R±{1 -t)n (R^fa) - tt/2) n (R^(b r ) + tt/2) 

over the interval [d,l\. 

Subject to the above three constraints the rectangle can everywhere be as big as one 
which will fit in the “small” part of the generalized cone. Thus 

((RHsi) - tt/ 2 ) n (R^(s r ) 4- tt/ 2 )) n v’(t) c v(t). 
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Furthermore, subject to V'(t), any orientation of A is valid which results in a bounding 
rectangle that is within the bounds given by the decreasing boundaries of the generalized 
cones. Recall that at each point of the spine, with parameter t, these boundaries have both 
left and right radius of mt -f c and that m < 0. Thus sufficient conditions are that 

R(0 + tt/2) < m X {t + R(0)) + c 
R{0 — tt/ 2) < m X (t + R{0)) + c 

are both true. These can be expressed as 

[R-k /2 — mR](0) < (mt -)- c) 

\R—x /2 — tnR](0) < [mt + c) 

whence 


(Pw /2 — + c) n [R—1,/2 — mR]- L (mt + c)) Pi V'[t) C F(<). 


In summary then, over the range t £ [d,/], a valid subset of orientations for A can be 
expressed as 


V[t) =R±(l — t)n (R x (6,) - tt/2) n (R X (M + tt/2) 
n (((R-^) - */2) n (R x (s r ) + tt/ 2 )) 

U ([Rtt/ 2 — mR]- L (mt c) ft \R—k /2 — -j- c))^ 

and elsewhere V[t) — 0. The key property of V(t) is the following. 


LEMMA: For t lt t 2 E [d,l], ti < t -2 implies V(t 2 ) C V[ti). 


Note that V[t) does not necessarily include all possible orientations for object A to 
be contained in cone C at point with paremeter t along the spine. However it the set of 
possibilities which the find-path algorithm described here is willing to consider. Its key 
property is that as stated by the lemma the set of valid orientations (amongst those to be 
considered) does not increase as the object is moved from the “big” end to the “small” end 
of a generalized cone. 




Figure 9. A path found by the algorithm. 
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Figure 10. A path found by the algorithm. 
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4. Searching for a Path 


The algorithm to find a collision free path proceeds as follows. First the generalized 
cones which describe free space are coomputcd (this can be done in time 0(n 3 )). Next the 
cones are pairwise examined to determine if and where their spines intersect. Up to this 
stage the algorithm is independent of the object A to be moved through the enviroinment. 
Each spine intersection point for each cone is now annotated with the subset of [0, 2x] 
which describes the orientations for the object A in which it is guaranteed to be completely 
contained in the generalized cone. The final stage of the algorithm is to find a path from the 
initial position to the goal position following the spines of the generalized cones and changing 
from cone to cone at spine intersection points. If the object is kept at valid orientations at 
each point on each spine then the path will be collision free. 

4.1 A searchable graph. 

A graph is built then searched with the A* algorithm (see Nilsson [7]). The cost function 
used is the distance traveled through space. 

The nodes of the graph consist of points on generalized cone spines which correspond 
to points of intersection, along with a single orientation subinterval of [0, 2x] (except that 
intervals may wrap around from 2x to 0). Thus for an intersection point with parameter t 
on the spine of cone C, a node for each interval in V(t) is built. 

The arcs in the graph arise in two ways; those that correspond to transfer from one 
intersection point on a spine to another, and those that correspond to transfer from one 
spine to another at their common intersection point. All such candidate pairs of nodes are 
checked for connectivity. The lemma of section 3 guarantees that it suffices to check if the 
orientation intervals of the two nodes, for intra-cone arcs, have a non-empty intersection. 
If so, then it is certainly possible to move the object A from one point to the other using 
any orientation within that intersection. For inter-cone pairs of nodes it also suffices to 
check for non-empty intersection of their orientation intervals, as the points are coincident 




in space. 


The graph can now be searched to find a path consisting of an ordered set of nodes 
from initial position to goal position. 

4.2 Intermixing rotations and translations. 

If the graph search is successful it indicates a whole class of collision free paths. It 
remains to chose one particular trajectory. 

The class of paths found share a common trajectory for the origin of the moving object. 
The orientation interval associated with each node of the path through the graph constrains 
the valid orientations at that point. It is also necessary to ensure that no invalid orientation 
is assumed during translation and rotation along the spine of a generalized cone. 

It may well be the case that rotations are expensive, and it is worth searching the class 
of paths resulting from the graph search to find the optimal path in terms of least rotation 
necessary. This could be carried out using the A* algorithm. Every node in the path found 
in the first search would be turned into a collection of nodes. Each pair of adjacent nodes 
in the path found above would have their orientation ranges intersected. Combine each 
end value of each intersection, and the initial and final orientations into a set of possible 
orientations. Each orientation which is valid for a particular path node, along with that 
node, generates a node in the new graph. Adjacency would be inherited from adjacency of 
generating nodes in the original path. The A* agorithm is used to search this new graph. 

Figures 9 and 10 illustrate two paths found by a much simpler method. Rotations 
are restricted to points corresponding to nodes of the path - i.e. the object is held with 
fixed orientation during each translation. The orientation at each point is the midpoint of 
the orientations given by intersecting the allowed orientation intervals of the corresponding 
path node, and the subsequent node. Thus traversal of intra-cone arcs of the graph built 
above are rotation free, and traversal of inter-cone arcs are pure rotations. The lemma of 




section 3 guarantees that this strategy leads to valid collision free paths. 

5. Usefulness and Applicability 

The major drawback of the find-path algorithm presented in this paper is that paths 
are restricted to follow the spines of the generalized cones chosen to represent free space. It 
typically does not work well in tightly constrained spaces as there are insufficient generalized 
cones to provide a rich choice of paths. Furthermore, in a tight space the interior points 
of spines are always near to the spine ends. Therefore the need to extend the bounding 
rectangles rearward to the maximum possible distance for the moving object (so that the 
lemma of section 3 will hold) means that typically there are very few points of intersections 
of generalized cones where the object’s bounding rectangle in each orientation is contained 
within the appropriate generalized cone. This situation could be improved significantly by 
developing a better algorithm for decomposing free space into generalized cones. The current 
pruning technique is often too drastic - if an obstacle intersects a generalized cone it may 
be better to simply reduce the cone radius, rather than the current practice of slicing out 
all of the spine onto which the obstacle projects. 

5.1 Advantages. 

In relatively uncluttered environments the algorithm is extremely fast. Furthermore it 
has the following properties which are absent from some or all of the algorithms mentioned 
in section 1. 

1. The paths found, and the amount of computation needed are completely independent of 
the chosen world coordinate system. 

2. Obstacles affect the representation of free space only in their own locality. Thus addi¬ 
tional obstacles spatially separated from a free path found when they are not present can 
not affect the ability to find that free path then they are present. (Resource limited, and 
approximating algorithms which cut free space into rectangloid cells often suffer from this 




problem.) 


3. Free space is represented explicitly rather than as the complement of forbidden space. 

4. The paths tend to be equally far from all objects, out in truly free space, rather than 
scraping as close as possible to obstacles, making the instantiation of the paths by mechanical 
devices subject to failure due to mechanical imperfections. 


We plan to use this algorithm in conjunction with an implementation of an algorithm 
([3]) which finds paths with rotations based on a configuration space approach. The general¬ 
ized cone method will be used to find the easy parts of the path (the other method is 
computationally expensive both for easy and hard paths) leaving the more expensive method 
for the hard parts. The algorithm presented here can also provide some direction for the 
hard parts as it can quickly compute all topologically possible paths. 

5.2 Three Dimensions. 

Clearly the eventual goal of this work is to develop algorithms which solve the find- 
path problem in three dimensions with three rotational degrees of freedom for each link of 
general articulated devices. In this section we discuss approaches to extending the presented 
algorithm to the case of a single convex polyhedron moving through a space of polyhedral 
obstacles. 

Three dimensional generalized cones can be constructed by considering all triples of 
faces of obstacle polyhedra. The construction algorithm (including intersection with obstacle 
polyhedra) has complexity 0(n 4 ) in the number of obstacle faces. The generalized cones 
so constructed will have triangular cross-sections. It is not clear how to pairwise intersect 
the generalized cones, as in general their spines will not intersect. The “path-class” idea 
presented below may solve this problem. 


Rather than a bounding rectangle, a right triangular prism should be used where the 




triangle is similar to the cross section of the generalized cone being traversed. If more than 
one rotational degree of freedom is assumed there may be problems inverting this bounding 
volume. Rather than V(t) having a set of intervals as a value, it will have a set of three— 
dimensional polyhedra. 

Both for the two dimensional and three dimensional problems it is worth investigating 
using paths other than the spine through a generalized cone. Then V is parameterized in 
terms of t and the two end-points of a whole class of paths through individual cones. This 
increases the complexity of the search but will lead to more paths than simply using the 
spine. 

5.3 Complexity Issues. 

In discussions with Tornas Lozano-Perez it became evident that there exists an 0(n 2 c^ ]og n ) 
algorithm which can find the same generalized cones as docs the 0(n 3 ) algorithm presented 
here. The spines used for generalized cones correspond to the Voronoi boundaries between 
obstacles (see Drysdale [4] for a complete solution to the problem), and they can be found in 
0(nc\/'° e n ) time. There remains a factor of O(n) to do the intersection of the hypothesized 
generalized cones with other obstacles. It is unlikely that such an algorithm would perform 
better than the 0(n 3 ) algorithm presented for practical sized problems. 

The complexity of pairwise intersecting the generalized cones was not addressed in the 
body of the paper. An upper bound of 0(n 4 ) can easily be obtained as there are at most 
0(n 2 ) generalized cones to be considered. However, it seems hard to find a constant where 
cn 2 generalized cones can be acheived constructively as the number of obstacle polygons is 
increased. For any c it seems that space eventually tends to become too cluttered for all 
the cones to be constructed. Thus it seems likely that a better bound than 0(n 4 ) exists. (In 
fact the Voronoi complexity above suggests a bound of 0(n 2 c 2 v /l °e n ),) 
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